﻿/*     导航抽象层
 * 导航算法接口（磁导航、坐标导航）
 * 路线设计、坐标点设计
 ****/
#include "navigator.h"

//#define GII_DEBUG

Navigator::Navigator( int argc, char** argv, Maindeal* maindeal, QObject* parent ) : QObject( parent ), maindeal( maindeal ), argc( argc ), argv( argv ) {}

Navigator::~Navigator() {}

const QList< Station >& Navigator::stations_list() {
    return sub_route.stations;
}

/**
 * @brief Navigator::slt50msTimer
 * 主要处理行走相关,包含二维码导航,避障,停车,日志等.
 */
void Navigator::slt50msTimer() {
    //  qDebug() << "步骤3----------------" <<endl;
    static int delay_counter = 0;  //用于延时
    // static int next_route_delay_counter = 0, auto_back_counter = 0;   //
    static int ros_error_counter = 0;                                                  //用于延时
    int        delay_time        = Param::reset_obs_stop_timer / Param::predict_time;  //避障恢复后等待时间

    //  int next_route_delay_time = Param::next_route_timer / Param::predict_time;
    int ros_error_restart_time = 1000 / Param::predict_time;  // ros出现异常重新恢复前等待时间
    int ros_error_cancel_time  = 4000 / Param::predict_time;  // ros出现异常累计时间足够后取消路线任务
    //获取参数
    getCanNaviParam();
    //计算坐标
    calcPosition();
    //获取当前id
    getSlamNaviCurId();
    //检测是否读取过二维码
    //  checkQrcodeRead();
    //切换壁障区域
    //  changeObs();
    /***************************************急停事件***************************************/
    if ( is_emergency_stop || ( maindeal->agvhardware()->getAgvState() == Agvhardware::AGV_EMERGENCY ) ) {  //硬件错误、急停按钮按下、is_emergency_stop=true
        EmergencyStop();
        is_auto_mode_enable = false;
        return;
    }
    /***************************************交管事件***************************************/
    else if ( maindeal->agvhardware()->getAgvState() == Agvhardware::AGV_TRAFFIC ) {  //交管触发
        EmergencyStop();
        return;
    }
    /***************************************避障触发***************************************/
    else if ( maindeal->agvhardware()->getAgvState() == Agvhardware::AGV_OBSTACLE ) {  //避障触发
        //决定延时时间
        delay_counter = delay_time;
        EmergencyStop();
        return;
    }
    //  auto_back_counter = 0;
    /***************************************触发延时***************************************/
    if ( delay_counter-- > 0 && is_auto_mode_enable ) {
        EmergencyStop();
        return;
    }
    else {
        delay_counter = 0;
    }
    /***************************************自动行驶模式***************************************/
    if ( is_auto_mode_enable && !is_emergency_stop ) {
        if ( navigation_mode == NAVIGATION_SLAM ) {
            // READY
            if ( slamNaviStep == READY ) {
                if ( slam_walkspeed != 0.0 || slam_veerrad != 0.0 ) slamNaviStep = TRACING;
            }

            // TRACING
            if ( slamNaviStep == TRACING ) {
                /****************实时弧度进入路线检测-2023.06.13*******************/
                int  targetid = sub_route.stations.back().id;                      //目标id
                bool result   = slam_route_palnning->isAnglejudgment( targetid );  //获取目标文件id
                if ( slam_dis_err_cur2target < 1500 && result )                    //路线距离是否小于1500 是否试是目标id
                                                                                   //          if(slam_dis_err_cur2target < 1500 && targetid ==3 )
                {
                    //                     qDebug()<< "进入路线距离:"<<slam_dis_err_cur2target<<endl;
                    //                     qDebug()<< "进入点位id:"<<sub_route.stations.back().id<<endl;
                    if ( fabs( slam_rad_err_start2target ) > Param::enter_angle_limit ) {
                        //                          qDebug()<< "进入路线距离角度:"<<slam_rad_err_start2target<<endl;
                        sigSendTips( "弧度实时检测异常-无法进入" );
                        setEmergencyStop( true );
                    }
                }
                /****************************************距离差和弧度差限制减速***************************************************/
                bool obs_slow_down = maindeal->agvhardware()->isObstacleSlowDown( isForward() );
                bool is_need_slow  = ( fabs( slam_point_to_line_distance ) >= 150 ||                                  //限制偏差距離
                                      fabs( slam_rad_err_start2target ) >= 0.15 ||                                    //限制弧度差
                                      ( slam_dis_start2target - slam_dis_err_cur2target <= Param::slow_down_dis ) ||  //限制起步前距离
                                      ( slam_navi_status == AROUND_THE_OBS ) ||                                       //限制绕障状态
                                      obs_slow_down )                                                                 //避障减速区域触发
                                        ? true
                                        : false;
                /****************************************发送减速***************************************************/
                static bool only_once1 = false, only_once2 = false;
                if ( is_need_slow && !only_once1 ) {
                    only_once1 = true;
                    only_once2 = false;
                    qnode_module->setForceSlow( true );  // DELETE setForceSlow 减速！
                    qDebug() << "setForceSlow:true";
                }
                else if ( !is_need_slow && !only_once2 ) {
                    only_once1 = false;
                    only_once2 = true;
                    qnode_module->setForceSlow( false );
                    qDebug() << "setForceSlow:false";
                }

                /****************************************UPDATE 2022.11.07
                         * QT自我判断到达，预防ros那边异常*************/
                // ros已经不发送速度，但是车还是TRACING步骤
                if ( slam_walkspeed == 0.0 && slam_veerrad == 0.0 && fabs( slam_dis_err_cur2target ) <= 100 ) {
                    ++ros_error_counter;
                    if ( ros_error_counter == ros_error_cancel_time ) {
                        qDebug() << "ros error!!--cancel the route";
                        qnode_module->cancel();
                    }
                    if ( ros_error_counter > ( ros_error_restart_time + ros_error_cancel_time ) ) {
                        ros_error_counter = 0;
                        qDebug() << "QT-GoalReached!!--ARRIVE";
                        slamNaviStep = ARRIVE;
                    }
                }
                else {
                    ros_error_counter = 0;
                }
                /****************************************设定slam速度***************************************************/
                fork_walk_setting_speed = slam_walkspeed;
                fork_veer_setting_rad   = slam_veerrad;
            }

            if ( slamNaviStep == ARRIVE ) {
                fork_walk_setting_speed = 0;
                fork_veer_setting_rad   = 0;
            }

            //处理激光导航步骤，决定下一步动作
            dealSlamNaviStep( slamNaviStep );
        }
    }
    //发送导航速度
    maindeal->agvhardware()->canModule()->CanSetSpeedAngle( fork_walk_setting_speed, fork_veer_setting_rad );
}

//关闭ROS
void Navigator::sltRosShutdown() {
    setEmergencyStop( true );
}

//接收到的单位是米，需要转换为毫米
void Navigator::sltcurrentPose( const QList< double >& pose ) {
    // qDebug() << "pose:" << pose;
    qnode_connect_time.start( 3000 );  //复位超时报警计时
    slam_point.setX( pose.at( 0 ) * 1000 );
    slam_point.setY( pose.at( 1 ) * 1000 );
    slam_point.setRadian( pose.at( 2 ) );
}

//接收到的单位 m/s,需要转化为 cm/s
void Navigator::slttargetSpeed( const QList< double >& speed ) {
    slam_walkspeed = speed.at( 0 );
    slam_veerrad   = speed.at( 1 );
}

//在到达目标
void Navigator::sltonGoalReached() {
    qDebug() << "ROS-GoalReached!!--cur2targetDis：" << slam_dis_err_cur2target;
    if ( fabs( slam_dis_err_cur2target ) <= 300 )  // ROS到达也加距离限制
    {
        qDebug() << "ROS-GoalReached!!--ARRIVE";
        slamNaviStep = ARRIVE;
    }
    else {
        qDebug() << "ROS-GoalReached, but slam_dis_err_cur2target > 300";
    }
}

// ROS定位丢失
void Navigator::sigonLocalizationLose() {
    sigSendTips( "错误，ROS可能发生定位丢失" );
    setEmergencyStop( true );
}

//导航状态
void Navigator::sltonNavStatus( const int& state ) {
    switch ( state ) {
    case -3:  //无可用规划速度
        slam_navi_status = NONE_SPEED;
        break;
    case -2:  //无可用路径
        slam_navi_status = NONE_ROUTE;
        emit sigSendTips( "警告：ros导航 无可用路径" );
        break;
    case -1:  //未知错误，一般不应该有
        slam_navi_status = ERROR;
        break;
    case 0:  //处于正常状态 （例如无障碍、有障碍但还能绕开等）
        slam_navi_status = NOMAL;
        break;
    case 1:  //局部目标点或者目标点被障碍物占据了，将走到障碍物面前并等待移除（停障）
        slam_navi_status = CANNONT2TARGET;
        break;
    case 2:  //避障模式下，识别到障碍物，但是暂时绕不开，将走到障碍物面前并等待移除（停障），或计算出新路径以绕开
        slam_navi_status = OBS_CANNONT2TARGET;
        break;
    case 3:  //停障模式下，识别到障碍物，将走到障碍物面前并等待移除
        slam_navi_status = STOP_CANNONT2TARGET;
        break;
    case 4:  //绕开障碍物
        slam_navi_status = AROUND_THE_OBS;
        break;
    }
}

/**
 * @brief Navigator::calcNextStation 计算下一个station
 */
bool Navigator::calcNextStation() {
    if ( navigation_mode == NAVIGATION_SLAM ) {
        if ( !isAllowSetRoute() ) {
            setEmergencyStop( true );
            return false;
        }

        bool is_done = calcSlamRoute();
        if ( is_done ) {
            EmergencyStop();  //导航完成先停车
            qDebug() << "进入到点位置等待------------" << endl;

            /**************************激光导航完成，待命点******************************/
            navigation_mode = NAVIGATION_NONE;
            maindeal->agvhardware()->setAgvState( Agvhardware::AGV_ARRIVE );
            emit sigSendTips( "到达待命路线" );

            /**************************充电站点，自动充电******************************/
            if ( getSlamId() != 99999 && getSlamId() > 0 && slamRoutePlanning()->isChargeNum( getSlamId() ) ) { maindeal->agvhardware()->startCharge(); }

            //      switch (slam_route_palnning->getFunMode(slam_cur_navi_route)) {
            //        case SlamRoutePlanning::STANDBY:
            //          maindeal->agvhardware()->setAgvState(Agvhardware::AGV_ARRIVE);
            //          emit sigSendTips("到达待命路线");
            //          break;
            //        case SlamRoutePlanning::ALLOW_TRANSMIT:
            //          if (is_slam_navi_last) {
            //            maindeal->agvhardware()->setAgvState(Agvhardware::AGV_ALLOW_TRANSMIT);
            //            emit sigSendTips("允许升降");
            //          } else {
            //            maindeal->agvhardware()->setAgvState(Agvhardware::AGV_ARRIVE);
            //            emit sigSendTips("到达待命路线-2");
            //          }
            //          break;
            //      }

            /**************************BEGIN，激光导航完成，待命点******************************/
            // TODE自动循环测试
            /*         navigation_mode = NAVIGATION_NONE;
                     qDebug() << "进入函数---------";
                     maindeal->agvhardware()->setAgvState(Agvhardware::AGV_ARRIVE);
                     // 切换到达状态
                     emit sigSendTips("到达待命路线");
                     QTimer::singleShot(1000, this, [=]() {
                       bool flag = false;
                       int wait_time = 1000;
                       switch (slam_cur_navi_id) {
                         case 4:
                         case 7:
                         case 10:
                           wait_time = 6000;
                           if (maindeal->agvhardware()->getForkPosState() == Agvhardware::FORK_TOP) {
                             maindeal->agvhardware()->setForkliftState(Agvhardware::FALL);
                             flag = true;
                           } else if (maindeal->agvhardware()->getForkPosState() == Agvhardware::FORK_BOTTOM) {
                             maindeal->agvhardware()->setForkliftState(Agvhardware::RAISE);
                             flag = true;
                           } else {
                             maindeal->agvhardware()->setForkliftState(Agvhardware::STOP);
                             flag = false;
                           }
                           break;
                         default: flag = true;
                       }
                       if (flag) {
                         QTimer::singleShot(wait_time, this, [=]() {
                           if (slam_cur_navi_route == 1) {
                             SystemNaviOrder(2, true);
                           } else if (slam_cur_navi_route == 2) {
                             SystemNaviOrder(3, true);
                           } else if (slam_cur_navi_route == 3) {
                             SystemNaviOrder(4, true);
                           } else if (slam_cur_navi_route == 4) {
                             SystemNaviOrder(1, true);
                           }
                         });
                       }
                     });*/
            /**************************END，激光导航完成，待命点******************************/
        }
        else {
            if ( !execSlamRoute() ) {
                setEmergencyStop( true );
                emit sigSendTips( "错误：SLAM路线执行失败" );
                return false;
            }
        }
        return true;
    }
    else {
        emit sigSendTips( "错误：未设定导航方式" );
        return false;
    }
}

// UPDATE 2022.09.04 获取到合适路线，并返回最小偏差值。
bool Navigator::inOnRoute( const int& num, int& err ) {
    QList< SlamRoutePlanning::StationModel > p2p      = slam_route_palnning->getStationModel( num );
    int                                      progress = p2p.size();
    // 寻找最近起点
    double mindis = 0.0;
    for ( int i = 0; i < progress; i++ ) {  //求出当前点到直线段距离最短到一段
        Point start_point           = p2p.at( i ).stations.front().point;
        int   target_stations_count = p2p.at( i ).stations.size();
        for ( int j = 1; j < target_stations_count; j++ ) {
            Point next_point = p2p.at( i ).stations[ j ].point;

            double dis_cur2start = Utility::calcDistance( slam_point, start_point );
            double dis_cur2end   = Utility::calcDistance( slam_point, next_point );
            double dis_start2end = Utility::calcDistance( start_point, next_point );
            double dis_err       = fabs( ( dis_cur2start + dis_cur2end ) - dis_start2end );

            // emit   sigSendTips( QString( " diserr:%1    i:%2" ).arg( dis_err, 0,
            // 'f', 2 ).arg( i ) );

            if ( i == 0 && j == 1 ) { mindis = dis_err; }
            else if ( dis_err < mindis || dis_err < dis_err_limit ) {  //最小距离并且是在直线上
                mindis = dis_err;
            }

            start_point = next_point;
        }
    }
    if ( mindis < Param::on_the_route_dis_limit ) {  // UPDATE 2022.07.11
                                                     // ontheroute距离改成参数模式
        err = static_cast< int >( mindis );
        return true;
    }
    return false;
}

bool Navigator::execSlamRoute() {
    if ( slam_cur_navi_progress >= slam_cur_navi_target_progress ) { return false; }

    //为了确保最后一段到终点坐标已经获取
    //  is_slam_navi_last = (slam_cur_navi_progress ==
    //  slam_cur_navi_target_progress - 1) //最后一段
    //                      &&
    //                      (slam_route_palnning->getFunMode(slam_cur_navi_route)
    //                      == SlamRoutePlanning::ALLOW_TRANSMIT);  //并且允许传输

    /**********打印*********/
    QString tmp_route   = QString( "加载路线ID:[%1" ).arg( sub_route.stations.front().id );
    QString tmp_route_2 = QString( "加载路线坐标:[(%1,%2)" ).arg( sub_route.stations.front().point.x() ).arg( sub_route.stations.front().point.y() );
    for ( int i = 1; i < sub_route.stations.size(); i++ ) {
        tmp_route.append( QString( "->%1" ).arg( sub_route.stations[ i ].id ) );
        tmp_route_2.append( QString( "->(%1,%2)" ).arg( sub_route.stations[ i ].point.x() ).arg( sub_route.stations[ i ].point.y() ) );
    }
    tmp_route.append( "]" );
    tmp_route_2.append( "]" );
    emit sigSendTips( tmp_route );
    emit sigSendTips( tmp_route_2 );
    /**********打印*********/

    /**********计算目标弧度*********/
    double rad_start2target = 0;
    if ( slam_cur_navi_progress < slam_cur_navi_target_progress - 1 ) {  //非最后一段
        // UPDATE 2022.08.11
        // 不再加载下一路段的弧度“+1”删除，直接完成到达快速加载下条路线。
        if ( current_station_models.at( slam_cur_navi_progress ).isforward ) {  //下一段的方向
            rad_start2target =
                Utility::calcRadian( ( current_station_models.at( slam_cur_navi_progress ).stations.end() - 2 )->point, current_station_models.at( slam_cur_navi_progress ).stations.back().point );
        }
        else {
            rad_start2target =
                Utility::calcRadian( current_station_models.at( slam_cur_navi_progress ).stations.back().point, ( current_station_models.at( slam_cur_navi_progress ).stations.end() - 2 )->point );
        }
    }
    else if ( slam_cur_navi_progress == slam_cur_navi_target_progress - 1 ) {  //最后一段
        //最后一段路角度修正，做一个新的表，存放终点的目标角度，不填就按默认线段的指向
        int    goalId = slam_route_palnning->getSlamRouteLastId( slam_cur_navi_route );
        double yaw;
        bool   result = slam_route_palnning->getgoalYaw( goalId, yaw );
        if ( result ) { rad_start2target = yaw; }
        else {                            //按表格目标弧度
            if ( sub_route.isforward ) {  //当前段的方向
                rad_start2target =
                    Utility::calcRadian( ( current_station_models.at( slam_cur_navi_progress ).stations.end() - 2 )->point, current_station_models.at( slam_cur_navi_progress ).stations.back().point );
            }
            else {
                rad_start2target =
                    Utility::calcRadian( current_station_models.at( slam_cur_navi_progress ).stations.back().point, ( current_station_models.at( slam_cur_navi_progress ).stations.end() - 2 )->point );
            }
        }
    }
    /**********计算目标弧度*********/

    std::vector< std::pair< double, double > > speed_limits = { { static_cast< double >( Param::min_distance_limit ) / 1000, static_cast< double >( Param::min_speed ) / 100 },
                                                                { static_cast< double >( Param::mid_distance_limit ) / 1000, static_cast< double >( Param::mid_speed ) / 100 },
                                                                { static_cast< double >( Param::max_distance_limit ) / 1000, static_cast< double >( Param::max_speed_2 ) / 100 },
                                                                { 1000.0, static_cast< double >( Param::max_speed ) / 100 } };  // UPDATE 2022.07.12 1000米以内都是高速

    std::vector< std::pair< double, double > > target_points;
    for ( int i = 1; i < sub_route.stations.size(); i++ ) {
        target_points.push_back( { sub_route.stations[ i ].point.x() / 1000.0, sub_route.stations[ i ].point.y() / 1000.0 } );
    }

    //设定起点终点
    qnode_module->setGoal( { sub_route.stations.front().point.x() / 1000.0, sub_route.stations.front().point.y() / 1000.0 }, target_points, rad_start2target, speed_limits,
                           2,                                   // -1.取消导航 0.缺省值 1.避障  2.停障  3.反光板对接
                           ( sub_route.isforward ) ? 1 : -1 );  // 设定运动方向 mode: 1.前进 -1.倒退 0.前进倒退均可

    slamNaviStep             = READY;
    is_first_time_arrive_sig = false;  // UPDATE 2022.11.07 设定路线就复位is_first_time_arrive_sig变量

    //准备计算下一个进度
    slam_cur_navi_progress++;

    return true;
}

bool Navigator::isOnRoutes( const QList< SlamRoutePlanning::StationModel >& station_models, int* index, double* dist, double* dth ) {
    // 寻找最近起点
    double min_dis  = 0.0;
    double d_theta  = 0.0;
    int    progress = 0;
    for ( int i = 0; i < station_models.size(); i++ ) {
        double dis_err;
        double dth_tmp;
        if ( isOnRoute( station_models[ i ], &dis_err, &dth_tmp ) ) {
            if ( i == 0 || dis_err < min_dis || dis_err < dis_err_limit ) {
                min_dis  = dis_err;
                progress = i;
                d_theta  = dth_tmp;
            }
        }
    }

    if ( index != nullptr ) { *index = progress; }

    if ( dist != nullptr ) { *dist = min_dis; }

    if ( dth != nullptr ) { *dth = d_theta; }

    return min_dis < Param::on_the_route_dis_limit;
}

bool Navigator::isOnRoute( const SlamRoutePlanning::StationModel& station_model, double* dist, double* dth ) {
    // 寻找最近起点
    double mindis = 0.0;

    if ( station_model.stations.size() < 2 ) {
        emit sigSendTips( QString( "错误，Navigator::isOnRoute: 不该发生的错误，路线点数量(%1)小于2" ).arg( station_model.stations.size() ) );
        return false;
    }

    //求出当前点到直线段距离最短到一段
    Point start_point = station_model.stations.front().point;

    for ( int j = 1; j < station_model.stations.size(); j++ ) {
        Point next_point = station_model.stations[ j ].point;

        double dis_cur2start = Utility::calcDistance( slam_point, start_point );
        double dis_cur2end   = Utility::calcDistance( slam_point, next_point );
        double dis_start2end = Utility::calcDistance( start_point, next_point );
        double dis_err       = fabs( ( dis_cur2start + dis_cur2end ) - dis_start2end );

        if ( j == 1 || dis_err < mindis ) {
            mindis = dis_err;

            if ( dth != nullptr ) {
                double yaw = std::atan2( next_point.y() - start_point.y(), next_point.x() - start_point.x() );
                double tmp = slam_point.radian() - yaw;
                if ( tmp > M_PI ) { tmp -= 2 * M_PI; }
                else if ( tmp < -M_PI ) {
                    tmp += 2 * M_PI;
                }
                *dth = tmp;
            }
        }  //最小距离

        start_point = next_point;
    }

    if ( dist != nullptr ) { *dist = mindis; }

    return mindis < Param::on_the_route_dis_limit;
}

//计算导航点
bool Navigator::calcSlamRoute() {
    // 起点
    slam_cur_navi_target_progress = current_station_models.size();
    if ( slam_cur_navi_progress == 0 ) {
        double mindis   = 0.0;
        int    progress = 0;
        if ( !isOnRoutes( current_station_models, &progress, &mindis ) ) {  //判断距离是否过长
            emit sigSendTips( QString( "错误，最小距离超过限定值，请移动到合适轨道！ "
                                       "当前:%1mm,限制:%2mm,当前路线段：%3 / %4" )
                                  .arg( mindis, 0, 'f', 2 )
                                  .arg( Param::on_the_route_dis_limit )
                                  .arg( progress + 1 )
                                  .arg( slam_cur_navi_target_progress ) );
            setEmergencyStop( true );
            return false;
        }

        slam_cur_navi_progress = progress;
        emit sigSendTips( QString( "最近路线段:%1/%2,距离:%3mm" ).arg( slam_cur_navi_progress ).arg( slam_cur_navi_target_progress ).arg( mindis ) );
        emit sigSendTips( QString( "激光导航开始,总路段长度:%2段" ).arg( slam_cur_navi_target_progress ) );
    }

    if ( slam_cur_navi_progress >= slam_cur_navi_target_progress ) { return true; }  // 路线完成

    sub_route = current_station_models.at( slam_cur_navi_progress );

    if ( !sub_route.isforward && maindeal->agvhardware()->getForkPosState() != Agvhardware::FORK_BOTTOM && maindeal->agvhardware()->getGoodsState() == Agvhardware::EMPTY ) {
        emit sigSendTips( QString( "错误，空载状态下，只允许货叉处于下限位时执行倒车" ) );
        setEmergencyStop( true );
    }

    //  qDebug() << "index: " << slam_cur_navi_progress;
    //  //为了确保最后一段到终点坐标已经获取
    //  if ((slam_cur_navi_progress == slam_cur_navi_target_progress - 1)
    //  //最后一段
    //      && (slam_route_palnning->getFunMode(slam_cur_navi_route) ==
    //      SlamRoutePlanning::ALLOW_TRANSMIT))  //并且允许传输
    //  {
    //    is_slam_navi_last = true;
    //  } else {
    //    is_slam_navi_last = false;
    //  }

    return false;
}
//无用参数
QMessageLogContext null_context;
/**
 * @brief Navigator::log
 * 把相关数据保持到log中.把qDebug流重定向到文件的程序在main.cpp
 */
void Navigator::sigLog() {
    auto    battery_data = maindeal->agvhardware()->Battery_Ropeencoder_canModule()->getBatteryData();
    QString navi_info( QString( "%1%,%2V,%3A,%4,%5,%6,%7,%8,%9," )
                           .arg( battery_data.soc )        // UPDATE 2022.07.12 日志加入电量百分比
                           .arg( battery_data.total_vol )  // UPDATE 2022.09.19 日志加入电压值
                           .arg( battery_data.total_cur )  // UPDATE 2022.09.23 日志加入电流值
                           .arg( slam_cur_navi_route )
                           .arg( slam_cur_navi_id )
                           .arg( ( sub_route.isforward ) ? "前进" : "后退" )
                           .arg( getAgvStateChinese() )
                           .arg( getNavigationModeChinese( maindeal->navigator()->getNavigationMode() ) )
                           .arg( getSlamNaviStatusChinese( slam_navi_status ) ) );  // UPDATE 2022.06.25 日志打印ros上传的状态

    QString speed_info(
        QString( "%1,%2,%3,%4," ).arg( fork_walk_setting_speed, 0, 'f', 3 ).arg( fork_veer_setting_rad, 0, 'f', 3 ).arg( fork_walk_reality_speed, 0, 'f', 3 ).arg( fork_veer_reality_rad, 0, 'f', 3 ) );

    /****************************************根据导航模式，显示对应的参数*********************************/
    NaviStep navistep = getSlamNaviStep();
    double   erad, eh, edis;
    maindeal->navigator()->getSlamParam( erad, eh, edis );
    QString coordinate_info( QString( "%1->%2,%3,%4,%5,%6,%7,%8,%9,%10,%11,%12," )
                                 .arg( sub_route.stations.front().id )
                                 .arg( sub_route.stations.back().id )
                                 .arg( QMetaEnum::fromType< NaviStep >().valueToKey( navistep ) )
                                 .arg( cur_point.x() )
                                 .arg( cur_point.y() )
                                 .arg( cur_point.radian(), 0, 'f', 3 )
                                 .arg( slam_point.x() )
                                 .arg( slam_point.y() )
                                 .arg( slam_point.radian(), 0, 'f', 3 )
                                 .arg( edis, 0, 'f', 3 )
                                 .arg( eh, 0, 'f', 3 )
                                 .arg( erad, 0, 'f', 3 ) );

    QString iostate( QString( "0x%1,0x%2,0x%3" )  //
                         .arg( maindeal->agvhardware()->ioModule()->getIointputA(), 8, 16, QLatin1Char( '0' ) )
                         .arg( 0, 8, 16, QLatin1Char( '0' ) )
                         .arg( maindeal->agvhardware()->ioModule()->getIoOutput(), 8, 16, QLatin1Char( '0' ) ) );

    //  qInfo().noquote() << navi_info << speed_info << coordinate_info <<
    //  iostate;

    logMessage( QtInfoMsg, null_context, QString( "%1%2%3%4" ).arg( navi_info ).arg( speed_info ).arg( coordinate_info ).arg( iostate ) );
}

/**
 * @brief Navigator::resetAllSwitchAndParam 复位所有开光和参数。
 */
void Navigator::resetAllSwitchAndParam() {
    is_auto_mode_enable         = false;
    navigation_mode             = NAVIGATION_NONE;
    is_obstacle_function_enable = true;
    is_obstacle_slowdown_enable = true;
    sub_route.isforward         = true;
    // is_need_redo_slam_route       = false;
    // coor_cur_navi_progress = 0;         //坐标导航的进度
    // coor_cur_navi_target_progress = 0;  //坐标导航的目标进度
    slam_cur_navi_route           = 0;      //激光导航的路线
    slam_cur_navi_id              = 99999;  //激光导航id
    slam_cur_navi_progress        = 0;      //激光导航的进度
    slam_cur_navi_target_progress = 0;      //激光导航的目标进度
    EmergencyStop();
    slamNaviStep = ADJUST;  //写其他会有bug，必须是一个没用的默认状态
    qnode_module->cancel();
    lockLocalization( false );
    /** BUG 2022.6.15
     * UPDATE 2022.06.15
     *复位后不能停车，估计变量的变化还是周期太快，导致设定速度被刷新，带着导航速度继续行走。
     * 所有加多一个定时，再次设定速度为0
     **/
    QTimer::singleShot( 500, this, [=]() { EmergencyStop(); } );
}

/**
 * @brief Navigator::clearEmergencyState 清除急停状态,并使能agv的功能
 */
void Navigator::clearEmergencyState() {
    is_emergency_stop = false;
}

/**
 * @brief Navigator::sltStartButtonHit
 * 可以把开关按钮连接到此槽,作为启动自动导航模式的按钮.
 */
void Navigator::sltStartButtonHit() {  //  Navigator 自动模式切换
    /*************************************************打开自动模式***************************************************************/
    if ( !is_auto_mode_enable && ( maindeal->agvhardware()->getAgvState() != Agvhardware::AGV_EMERGENCY ) ) {
#ifndef GII_DEBUG
        switch ( maindeal->agvhardware()->getForkPosState() ) {
        case Agvhardware::FORK_UNKNOWN:
        case Agvhardware::FORK_RAISING:
        case Agvhardware::FORK_FALLING:
            emit sigSendTips( "货叉未复位，请手动复位至状态位" );
            return;
        case Agvhardware::FORK_BOTTOM:
        case Agvhardware::FORK_TOP:
            break;
        }
#endif

        is_auto_mode_enable = true;
        emit sigSendTips( "人工操作-自动模式打开" );
        EmergencyStop();  //打开自动模式也触发一次停车

        //激光导航模式恢复
        if ( navigation_mode == NAVIGATION_SLAM ) {
            if ( slam_cur_navi_route != 0 ) {
                navigation_mode = NAVIGATION_NONE;                                //重设路线前参数复位
                maindeal->agvhardware()->setAgvState( Agvhardware::AGV_ARRIVE );  //重设路线前参数复位
                emit sigSendTips( QString( "自动重设路线:%1" ).arg( slam_cur_navi_route ) );
                if ( slam_cur_navi_route == -1 ) {
                    if ( !SystemNaviOrder3( SystemNaviOrder3_config.start_id, SystemNaviOrder3_config.goal_id, SystemNaviOrder3_config.route_type, is_auto_set_route ) ) {
                        is_auto_mode_enable = false;                                    //重设路线失败，参数复位
                        maindeal->agvhardware()->setAgvState( Agvhardware::AGV_STOP );  //重设路线失败，参数复位
                    }
                }
                else {
                    if ( !SystemNaviOrder( slam_cur_navi_route, is_auto_set_route ) ) {
                        is_auto_mode_enable = false;                                    //重设路线失败，参数复位
                        maindeal->agvhardware()->setAgvState( Agvhardware::AGV_STOP );  //重设路线失败，参数复位
                    }
                }
            }
        }
        //无导航模式
        else if ( navigation_mode == NAVIGATION_NONE ) {
            maindeal->agvhardware()->setAgvState( Agvhardware::AGV_ARRIVE );  // 切换到达状态
        }
    }
    /*************************************************关闭自动模式***************************************************************/
    else if ( is_auto_mode_enable ) {
        is_auto_mode_enable = false;  //自动模式关闭的时候，一定要执行停车,并且关闭所有功能
        EmergencyStop();
        // UPDATE 2022.06.15
        // 关闭自动模式后不能停车，估计变量的变化还是周期太快，导致设定速度被刷新，带着导航速度继续行走。
        QTimer::singleShot( 500, this, [=]() { EmergencyStop(); } );
        //    maindeal->agvhardware()->setChargeEnable(false);
        maindeal->agvhardware()->stopCharge();
        maindeal->agvhardware()->setForkliftState( Agvhardware::STOP );
        maindeal->agvhardware()->setAgvState( Agvhardware::AGV_STOP );  // 切换停止状态
        emit sigSendTips( "人工操作-自动模式关闭，并停车!" );
    }
}

//获取CAN导航参数
void Navigator::getCanNaviParam() {
    maindeal->agvhardware()->canModule()->GetCanRecivNaviParam( &fork_walk_reality_speed, &fork_veer_reality_rad );  // can recive
    // TODO qnode uploadSpeed
    qnode_module->uploadSpeed( fork_walk_reality_speed, fork_veer_reality_rad );
}

//  Navigator 计算坐标
void Navigator::calcPosition() {
    /*********************************二激光导航参数计算********************************************/
    if ( navigation_mode == NAVIGATION_SLAM ) {
        double mindis = 0.0;
        double length = 0.0;

        int near = 1;

        Point start_point           = sub_route.stations.front().point;
        int   target_stations_count = sub_route.stations.size();
        for ( int j = 1; j < target_stations_count; j++ ) {
            Point next_point = sub_route.stations[ j ].point;

            double dis_cur2start = Utility::calcDistance( slam_point, start_point );
            double dis_cur2end   = Utility::calcDistance( slam_point, next_point );
            double dis_start2end = Utility::calcDistance( start_point, next_point );
            double dis_err       = fabs( ( dis_cur2start + dis_cur2end ) - dis_start2end );

            length += dis_start2end;

            if ( j == 1 ) {
                mindis = dis_err;
                near   = j;
            }
            else if ( dis_err < mindis ) {
                mindis = dis_err;
                near   = j;
            }

            start_point = next_point;
        }

        // 计算剩余路线长度
        double dis_err_cur2target = Utility::calcDistance( slam_point, sub_route.stations[ near ].point );
        start_point               = sub_route.stations[ near ].point;
        for ( int j = near; j < target_stations_count; j++ ) {
            Point next_point = sub_route.stations[ j ].point;
            dis_err_cur2target += std::hypot( start_point.x() - next_point.x(), start_point.y() - next_point.y() );
            start_point = next_point;
        }

        slam_point_to_line_distance = mindis;  // Utility::calcPointToLineDistance( slam_point,
                                               // *start_station.point(), *target_station.point() );
        slam_rad_err_start2target = Utility::calcRadErrStart2Target( slam_point, ( sub_route.stations.end() - 2 )->point, sub_route.stations.back().point, sub_route.isforward );

        // TODO 2023.06.10 需要下周测试
        //    double radtaget=Utility::calcRadian((sub_route.stations.end() - 2)->point, sub_route.stations.back().point);
        //    qDebug()<<"(sub_route.stations.end() - 2)->point:"<<(sub_route.stations.end() - 2)->point;
        //    qDebug()<<" sub_route.stations.back().point:"<< sub_route.stations.back().point;
        //    qDebug()<<"radtaget:"<<radtaget;

        slam_dis_err_cur2target = dis_err_cur2target;  // Utility::calcDistance( slam_point,
                                                       // *stations.back().point() );
        slam_dis_start2target = length;                // Utility::calcDistance( *start_station.point(),
                                                       // *target_station.point() );
    }
}

//开始SLAM导航
void Navigator::slamNaviStart() {
    maindeal->agvhardware()->canModule()->AccDecUpdate( Param::jog_acc, Param::jog_dec );  //  4.8 加减速度在线修改
    slam_cur_navi_progress        = 0;
    slam_cur_navi_target_progress = 0;
    navigation_mode               = NAVIGATION_SLAM;
    is_obstacle_function_enable   = true;
    is_obstacle_slowdown_enable   = true;

    if ( !is_auto_mode_enable ) {
        emit sigSendTips( "错误：非自动模式，无法启动导航!" );
        return;
    }

    // 切换行走状态
    maindeal->agvhardware()->setAgvState( Agvhardware::AGV_RUNNING );
    calcNextStation();
}

//急停
void Navigator::EmergencyStop() {
    fork_walk_setting_speed = 0;
    fork_veer_setting_rad   = 0;
    //发送导航速度
    maindeal->agvhardware()->canModule()->CanSetSpeedAngle( 0, 0 );

    maindeal->agvhardware()->setForkliftState( Agvhardware::STOP );
}

//设置SLAM位置
void Navigator::setSlamPose( const Point& point ) {
    slam_point = point;
}

//处理SLAM导航步骤
void Navigator::dealSlamNaviStep( const NaviStep& step ) {
    // UPDATE 2022.11.07 修改ros上传到达信号的防重复方法
    switch ( step ) {
    case ARRIVE: {
        // UPDATA 2023-03-19 根据当前id判断需要的目标弧度，弧度精度不达标，自行修正角度
        bool is_rad_ok = false;
        //          double raderr = 999.99;
        double targetrad;

        int    num       = 0;
        double backspeed = 0;

        if ( !is_first_time_arrive_sig ) {
            //判断当前id是不是ini文件goalYaw表里面需要判断的id
            if ( slam_route_palnning->getgoalYaw( slam_cur_navi_id, targetrad ) ) {
                if ( targetrad - Param::radian_precision < slam_point.radian() && slam_point.radian() < targetrad + Param::radian_precision ) { is_rad_ok = true; }
                else if ( targetrad + Param::radian_precision < slam_point.radian() ) {
                    fork_veer_setting_rad   = -1.571;
                    fork_walk_setting_speed = 0.02;
                }
                else if ( targetrad - Param::radian_precision > slam_point.radian() ) {
                    fork_veer_setting_rad   = +1.571;
                    fork_walk_setting_speed = 0.02;
                }
            }
            else {
                is_rad_ok = true;
            }
            //到达位置后后退转角修正补偿-2023.06.28
            if ( slam_route_palnning->getbackpoint( slam_cur_navi_id, num, backspeed ) ) {
                qnode_module->backStepset( num, backspeed );
                qDebug() << "num:" << num << "backspeed:" << backspeed << endl;
                is_rad_ok = true;
            }
            else {
                is_rad_ok = true;
            }
        }

        //                  raderr = slam_point.radian() - targetrad;
        //                  Utility::radianCorrection( &raderr);
        //                  qDebug() << "获取当前id点位是" << slam_cur_navi_id;
        //                  qDebug() << "获取当前id目标弧度范围差为" << targetrad;
        //                  qDebug() << "当前目标弧度值：" << slam_point.radian();
        //                   qDebug() << "当前目标转为弧度值：" << tempradvalue;
        //                  qDebug() << "目标弧度差值为：" << raderr;
        //                  qDebug() << "当前is_rad_ok状态为" << is_rad_ok;
        //              }
        //              else
        //                  //若果角度满足限制需求，is_rad_ok就直接OK进入到站停止
        //              {
        //                 is_rad_ok = true;
        //              }
        //判断弧度差
        //              if( !is_rad_ok && fabs( raderr) > Param::radian_precision){
        //                  if(raderr > 0.0 ){
        //                      //右转要摆的弧度
        //                      double targetrad = 1.57;
        //                      fork_veer_setting_rad = targetrad;
        //                      raderr = fork_veer_reality_rad - targetrad;
        //                      Utility::radianCorrection ( &raderr);
        //                      if( fabs( raderr ) <= Param::radian_precision) fork_walk_setting_speed = 0.02;
        //                  }
        //                  else if ( raderr < 0.0 ){
        //                      double targetrad = -1.57 ;
        //                      fork_veer_setting_rad = targetrad;
        //                      raderr = fork_veer_reality_rad - targetrad;
        //                      Utility::radianCorrection ( &raderr );
        //                      if ( fabs( raderr ) <= Param::radian_precision ) fork_walk_setting_speed = 0.02;

        //                  }
        //              }
        //              else {
        //                  //摆正 停止运动
        //                    is_rad_ok = true;
        //                    maindeal->navigator()->setForkVeersettingrad( 0 );
        //                    maindeal->navigator()->setForkWalksettingspeed( 0 );

        //              }
        //          }

        if ( is_rad_ok && !is_first_time_arrive_sig ) {
            bool ready_to_next = true;

            switch ( sub_route.station_fun ) {
            case SlamRoutePlanning::GOON:
                break;

            case SlamRoutePlanning::FORK_UP:
                if ( maindeal->agvhardware()->getForkPosState() != Agvhardware::FALL ) {
                    ready_to_next = false;
                    maindeal->agvhardware()->setForkliftState( Agvhardware::RAISE );
                }
                break;

            case SlamRoutePlanning::FORK_DOWN_IF_EMPTY:
                if ( maindeal->agvhardware()->getGoodsState() == Agvhardware::EMPTY && maindeal->agvhardware()->getForkPosState() != Agvhardware::FORK_BOTTOM ) {
                    ready_to_next = false;
                    maindeal->agvhardware()->setForkliftState( Agvhardware::FALL );
                }
                break;
            }

            if ( ready_to_next && calcNextStation() ) {
                qDebug() << "ARRIVE---calcNextStation";
                is_first_time_arrive_sig = true;
            }
        }
    } break;

    case NAVI_ERROR: {
        emit sigSendTips( "错误，激光导航错误!" );
        is_emergency_stop        = true;
        is_first_time_arrive_sig = false;
    } break;

    case ADJUST:
        is_first_time_arrive_sig = false;
        break;

    case TRACING:
        is_first_time_arrive_sig = false;
        break;

    case READY:
        is_first_time_arrive_sig = false;
        break;
    }
}

/**
 * @brief Navigator::changeObs
 * obs.csv:使用slampoint坐标加slam弧度强制切换避障区域
 * slamobs.csv使用激光导航路线段，强制切换避障区域
 * 如果没有强制切换，按默认自动切换。自动切换的区域按参数表变换
 */
// UPDATE 2022.09.23  changeObs更新 修改激光导航切换条件
void Navigator::changeObs() {
    /*******************************************自动模式最高优先级强制切换obs.csv**************************************************/
    int  front = 3, back = 3;
    bool l_obs = true, r_obs = true;  // UPDATE 2022.11.01 bos加入左右壁障开关,默认打开
    bool result = slam_route_palnning->isNeedChangeObs( slam_point, &front, &back, &l_obs, &r_obs );
    maindeal->agvhardware()->setL_R_obs_open( l_obs, r_obs );
    if ( result ) {
        front_obs = front;
        back_obs  = back;
        maindeal->agvhardware()->ChangeObs( front_obs, front_obs, back_obs, back_obs );
        return;
    }
    /*******************************************手动模式***********************************************/
    if ( !is_auto_mode_enable ) {
        front_obs = 1;
        back_obs  = 1;
    }
    else {  // TODO 自动模式避障切换问题
        /*******************************************激光导航模式的避障切换************************************************************/
        if ( navigation_mode == NAVIGATION_SLAM ) {
            // UPDATE 2022.09.22 slam—obs文件特殊切换避障
            int  front = 3, back = 3;
            bool result = slam_route_palnning->isNeedChangeSlamObs( sub_route.stations.front().id, sub_route.stations.back().id, &front, &back );
            if ( result ) {
                front_obs = front;
                back_obs  = back;
            }
            else {
                //用设定的线速度来变换避障，如果用实际的速度，因有加减速度，反应较慢，不适合。
                // int v_value = static_cast< int >( ( left_setting_speed +
                // right_setting_speed ) / 2 );
                //用实际速度切换避障区域
                int v_value            = static_cast< int >( get_Fork_WalkRealitySpeed() * 100 );
                int change_speed_value = 10;  //单位 cm/s
                //每次从较大的避障切换成小的，需要维持足够设定时间再切换回大的。
                static bool change_switch  = true;
                static int  delay_count    = 0;
                static int  last_front_obs = 0;
                int         delay_time     = ( Param::change_obs_timer + Param::reset_obs_stop_timer ) / Param::predict_time;
                if ( !change_switch && ( ++delay_count >= delay_time ) ) {
                    change_switch = true;
                    delay_count   = 0;
                }
                if ( sub_route.isforward ) {
                    if ( change_switch ) front_obs = ( v_value >= change_speed_value ) ? Param::slam_mode_default_max : Param::slam_mode_default_min;
                    if ( ( last_front_obs > front_obs ) && front_obs == Param::slam_mode_default_min ) { change_switch = false; }
                    last_front_obs = front_obs;
                    // UPDATE 2022.09.13 即将进行原地旋转才切换最小避障
                    if ( ( v_value < Param::min_speed ) &&              //线速度低
                         fabs( slam_point_to_line_distance ) <= 150 &&  //在路线附近
                         fabs( slam_rad_err_start2target ) > 0.17 ) {   //角度大于10°
                        front_obs     = 1;
                        change_switch = true;  //如果切换成小避障1，立马终止避障切换计时。按车实际间需求切换大避障
                        delay_count   = 0;
                    }
                    back_obs = 1;
                }
                else {
                    back_obs  = ( v_value <= -change_speed_value ) ? Param::slam_mode_default_max : Param::slam_mode_default_min;
                    front_obs = 1;
                }
            }
        }
    }
    maindeal->agvhardware()->ChangeObs( front_obs, front_obs, back_obs, back_obs );
}

//获取SLAM导航id
void Navigator::getSlamNaviCurId() {
    int id           = slam_route_palnning->getSlamPointId( slam_point );
    slam_cur_navi_id = id;
}

//获取避障切换情况，用于界面显示
void Navigator::getObsState( int& front, int& back ) {
    front = front_obs;
    back  = back_obs;
}

//获取导航参数
void Navigator::getSlamParam( double& Erad, double& Eh, double& Edis ) {
    Erad = slam_rad_err_start2target;
    Eh   = slam_point_to_line_distance;
    Edis = slam_dis_err_cur2target;
}

//获取导航当前路线长度
double Navigator::getSlamRouteLength() {
    return slam_dis_start2target;
}

//设置初始化位置(锁定slam定位，只在小车附近求解定位)
void Navigator::lockLocalization( bool sw ) {
    if ( is_init_pose_done == sw ) { return; }
    qnodeModule()->lockLocation( sw );
    qDebug() << "lock localization: " << sw;
    is_init_pose_done = sw;
}

bool Navigator::isAllowSetRoute() {
    //判断设定路线前提
    if ( !is_auto_mode_enable ) {
        emit sigSendTips( "警告，没打开自动模式，无法执行路线指令" );
        return false;
    }

    //   if ( ( navigation_mode == NAVIGATION_COOR ) || ( navigation_mode ==
    //   NAVIGATION_SLAM ) ) {
    //       emit sigSendTips( "提示，坐标导航运动中，无法执行路线指令" );
    //       return false;
    //   }

    //  if (isModeDucking()) {
    //    emit sigSendTips("警告，当前模式为进入机台对接，无法执行路线指令");
    //    return false;
    //  }

    // UPDATE 2022.06.06 解除导航中切换新路线 状态限制
    Agvhardware::AgvState agv_state = maindeal->agvhardware()->getAgvState();
    if ( ( agv_state != Agvhardware::AGV_ARRIVE ) && ( agv_state != Agvhardware::AGV_RUNNING ) ) {
        emit sigSendTips( "警告，设备状态不正确，无法执行路线指令" );
        return false;
    }

    Agvhardware::ForkPosState fork_state = maindeal->agvhardware()->getForkPosState();
    if ( ( fork_state != Agvhardware::FORK_TOP ) && ( fork_state != Agvhardware::FORK_BOTTOM ) ) {
        emit sigSendTips( "警告，设备状态不正确，无法执行路线指令" );
        return false;
    }

    return true;
}

// is自动设定路线
bool Navigator::isAutoSetRoute() {
    return is_auto_set_route;
}

// 初始化位置
bool Navigator::isInitPose() {
    return is_init_pose_done;
}

/**
 * @brief Navigator::SystemNaviOrder 收到调度系统的路线导航命令
 */
bool Navigator::SystemNaviOrder( int route, bool isAutoRoute ) {  //  Navigator 调度路线设定处理
    // 判断设定路线前提
    if ( !isAllowSetRoute() ) { return false; }

    // 判断路线情况
    if ( !slam_route_palnning->isSlamRoutes( route ) ) {
        emit sigSendTips( QString( "警告，不存在路线:%1，无法执行路线指令" ).arg( route ) );
        return false;
    }

    auto station_models = slam_route_palnning->getStationModel( route );

    if ( station_models.size() < 1 ) {
        emit sigSendTips( QString( "警告，路线长度不足，长度：%1，无法执行路线指令" ).arg( station_models.size() ) );
        return false;
    }

    emit sigSendTips( QString( "正确设定路线:%1号线" ).arg( route ) );
    slam_cur_navi_route = route;
    is_auto_set_route   = isAutoRoute;

    current_station_models = station_models;

    slamNaviStart();

    lockLocalization( true );
    return true;
}

/**
 * @brief Navigator::SystemNaviOrder2 收到调度系统的终点ID导航命令
 */
bool Navigator::SystemNaviOrder2( int goal_id, SlamRoutePlanning::RouteType route_type, bool isAutoRoute ) {
    ID  start_id;
    int dist;
    if ( !slam_route_palnning->findResentSlamPoint( slam_point, start_id, dist ) ) {
        emit sigSendTips( QString( "警告，未找到附近起始站点，无法执行路线指令，目标点:%1 [%2]" )  //
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( goal_id ) )
                              .arg( goal_id ) );
        return false;
    }

    return SystemNaviOrder3( start_id, goal_id, route_type, isAutoRoute );
}

/**
 * @brief Navigator::SystemNaviOrder3 收到调度系统的起点ID,终点ID导航命令
 */
bool Navigator::SystemNaviOrder3( int start_id, int goal_id, SlamRoutePlanning::RouteType route_type, bool isAutoRoute ) {
    // 判断设定路线前提
    if ( !isAllowSetRoute() ) { return false; }

    // 确保货叉处于正确位置
    switch ( route_type ) {
    case SlamRoutePlanning::NORMAL:  // 常规路线需要抬起货叉 至低位
        maindeal->agvhardware()->ChangeObs( 1, 1, 1, 1 );
        qnode_module->buttonSetboolAngle( true );  //反馈给ROS切换为angle_costs
        if ( maindeal->agvhardware()->getForkPosState() != Agvhardware::FORK_TOP ) {
            emit sigSendTips( QString( "警告，货叉未处于上限，无法执行一般路线指令" ) );
            return false;
        }

        break;
    case SlamRoutePlanning::ENTER:  // 进入站点且空载时，需要放下货叉 至低低位
        maindeal->agvhardware()->ChangeObs( 3, 3, 3, 3 );
        qnode_module->buttonSetboolAngle( false );  //反馈给ROS切换为 angle_costs1
        if ( maindeal->agvhardware()->getGoodsState() == Agvhardware::EMPTY && maindeal->agvhardware()->getForkPosState() != Agvhardware::FORK_BOTTOM ) {
            emit sigSendTips( QString( "警告，空载时货叉未处于低位，无法执行进入站点指令" ) );
            return false;
        }
        break;
    case SlamRoutePlanning::EXIT:
        break;
    }

    // 判断起始点情况
    if ( !slam_route_palnning->isIdVaild( start_id ) ) {
        emit sigSendTips( QString( "警告，不存在起始点:%1[%2]，无法执行路线指令" )  //
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( start_id ) )
                              .arg( start_id ) );
        return false;
    }

    // 判断终点情况
    if ( !slam_route_palnning->isGoalVaild( goal_id ) ) {
        emit sigSendTips( QString( "警告，不存在目标点:%1[%2]，无法执行路线指令" )  //
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( goal_id ) )
                              .arg( goal_id ) );
        return false;
    }

    // 判断起始点与目标点相同
    if ( start_id == goal_id ) {
        emit sigSendTips( QString( "警告，起始点与目标点相同:%1[%2]，无法执行路线指令" )  //
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( start_id ) )
                              .arg( start_id ) );
        return false;
    }

    QList< SlamRoutePlanning::StationModel > stantion_models = slam_route_palnning->caluStatuonModel( start_id, goal_id, route_type );

    if ( stantion_models.empty() ) {
        emit sigSendTips( QString( "警告，未计算出可用路线模型，无法执行路线指令，起始点:%1[%2], "
                                   "目标点:%3[%4]" )  //
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( start_id ) )
                              .arg( start_id )
                              .arg( maindeal->navigator()->slamRoutePlanning()->getMachineMsg( goal_id ) )
                              .arg( goal_id ) );
        return false;
    }

    double mindis;
    double dth;

    // 判断小车在路线上
    if ( !isOnRoutes( stantion_models, nullptr, &mindis, &dth ) ) {
        emit sigSendTips( QString( "错误，最小距离超过限定值，请移动到合适轨道！ 当前:%1mm,限制:%2mm" ).arg( mindis, 0, 'f', 2 ).arg( Param::on_the_route_dis_limit ) );
        setEmergencyStop( true );
        return false;
    }

    //  if (abs(dth) > M_PI_2) {
    //    emit
    //    sigSendTips(QString("错误，小车方向与轨道方向夹角（%1度）大于90度！").arg(abs(dth)
    //    / M_PI * 180, 0, 'f', 1)); setEmergencyStop(true); return false;
    //  }

    QString route( "" );
    for ( auto model : stantion_models ) {
        for ( auto station : model.stations ) {
            route += QString( "->%1" ).arg( station.id );
        }
    }
    emit sigSendTips( QString( "设定自动计算路线:%1" ).arg( route ) );

    // -1: auto route flag
    slam_cur_navi_route = -1;

    SystemNaviOrder3_config.start_id   = start_id;
    SystemNaviOrder3_config.goal_id    = goal_id;
    SystemNaviOrder3_config.route_type = route_type;
    is_auto_set_route                  = isAutoRoute;

    current_station_models = stantion_models;

    slamNaviStart();
    lockLocalization( true );

    return true;
}

//允许充电
bool Navigator::isAllowOpenCharge() const {
    return slam_route_palnning->isChargeNum( slam_cur_navi_id );
}

// /**
// * @brief Navigator::isNearTargetStation
// * 是否接近终点，用于判断小车是否接近对接口机台坐标！（用的是二维码构建的坐标系坐标）
// */
// bool Navigator::isNearTargetStation() const {
//  if (!isModeDucking())  //不是二维码对接模式，返回false
//  {
//    return false;
//  }
//  double value = Utility::calcDistance(cur_point, *stations.back().point());
//  if (fabs(value) <= 50.0)  //小于50mm
//  {
//    return true;
//  }
//  return false;
//}

int Navigator::getSlamRoute() const {
    return slam_cur_navi_route;
}

int Navigator::getSlamId() const {
    return slam_cur_navi_id;
}

int Navigator::getSlamStartId() const {
    return slam_start_navi_id;
}

int Navigator::getSlamGoalId() const {
    if ( current_station_models.empty() ) { return 0; }
    else if ( current_station_models.back().stations.empty() ) {
        return 0;
    }
    else {
        return current_station_models.back().stations.back().id;
    }
}

int Navigator::getSlamCurProgress() const {
    return slam_cur_navi_progress;
}

int Navigator::getSlamTagProgress() const {
    return slam_cur_navi_target_progress;
}

Navigator::NavigationMode Navigator::getNavigationMode() const {
    return navigation_mode;
}

///////////////////////////////
double Navigator::get_Fork_VeerSettingRad() const {
    return fork_veer_setting_rad;
}

double Navigator::get_Fork_WalkSettingSpeed() const {
    return fork_walk_setting_speed;
}

double Navigator::get_Fork_VeerRealityRad() const {
    return fork_veer_reality_rad;
}

double Navigator::get_Fork_WalkRealitySpeed() const {
    return fork_walk_reality_speed;
}

void Navigator::setForkWalksettingspeed( double value ) {
    fork_walk_setting_speed = value;
}

void Navigator::setForkVeersettingrad( double value ) {
    fork_veer_setting_rad = value;
}
////////////////////////////////

/**
 * @brief Navigator::showRoute 显示路线
 * 因为label显示空间有限，已经走过的站点不再显示，所以路线的显示随着进度越来越来短。
 */
QString Navigator::showRoute() const {
    QString route = QString( "  激光导航路线:%1号线 [总长度:%2 当前:%3]" ).arg( slam_cur_navi_route ).arg( slam_cur_navi_target_progress ).arg( slam_cur_navi_progress );

    return route;
}

SlamRoutePlanning* Navigator::slamRoutePlanning() const {
    return slam_route_palnning;
}

qnode::QNode* Navigator::qnodeModule() const {
    return qnode_module;
}

Navigator::SlamNaviStatus Navigator::getSlamNaviStatus() const {
    return slam_navi_status;
}

QString Navigator::getAgvStateChinese() {
    QString str = "";
    switch ( maindeal->agvhardware()->getAgvState() ) {
    case Agvhardware::AGV_RUNNING:
        str = "AGV行驶";
        break;
    case Agvhardware::AGV_ARRIVE:
        str = "AGV到站";
        break;
    case Agvhardware::AGV_LIFT_STOP:
        str = "AGV升降暂停";
        break;
    case Agvhardware::AGV_PRECHARGE:
        str = "AGV充电准备";
        break;
    case Agvhardware::AGV_CHARGING:
        str = "AGV充电";
        break;
    case Agvhardware::AGV_TRAFFIC:
        str = "AGV交管";
        break;
    case Agvhardware::AGV_STOP:
        str = "AGV停止";
        break;
    case Agvhardware::AGV_OBSTACLE:
        str = "AGV停障";
        break;
    case Agvhardware::AGV_EMERGENCY:
        str = "AGV急停";
        break;
    case Agvhardware::AGV_FORK:
        str = "AGV升降操作中";
        break;
    }
    //    case Agvhardware::AGV_ALLOW_TRANSMIT: str = "AGV允许传输"; break;
    return str;
}

QString Navigator::getNavigationModeChinese( Navigator::NavigationMode num ) {
    QString str = "";
    switch ( num ) {
    case NAVIGATION_NONE:
        str = "无导航";
        break;
    case NAVIGATION_SLAM:
        str = "激光模式导航";
        break;
    }
    return str;
}

QString Navigator::getSlamNaviStatusChinese( Navigator::SlamNaviStatus num ) {
    QString str = "";
    switch ( num ) {
    case NONE_SPEED:
        str = "无可用规划速度";
        break;
    case NONE_ROUTE:
        str = "无可用路径";
        break;
    case ERROR:
        str = "未知错误";
        break;
    case NOMAL:
        str = "处于正常状态";
        break;
    case CANNONT2TARGET:
        str = "目标点被障碍物占据";
        break;
    case OBS_CANNONT2TARGET:
        str = "避障模式下，绕不开障碍物";
        break;
    case STOP_CANNONT2TARGET:
        str = "停障模式下，绕不开障碍物";
        break;
    case AROUND_THE_OBS:
        str = "绕开障碍物";
        break;
    }
    return str;
}

/**
 * @brief Navigator::sltNavigatorInit
 * 初始化数据
 */
void Navigator::sltNavigatorInit() {
    // UPDATE 2022.07.22 对起点和终点Station变量进行初始化
    Station start_init;
    sub_route.stations = { start_init, start_init };

    //激光导航路径
    slam_route_palnning = new SlamRoutePlanning( this );
    slam_route_palnning->allFileReset();

    // TODO qnode slam导航初始化
    qnode_module              = new qnode::QNode();
    qnode::QNodelet* qNodelet = new qnode::QNodelet( qnode_module );
    connect( qnode_module, SIGNAL( sigSendTips( QString ) ), this, SIGNAL( sigSendTips( QString ) ) );
    connect( qnode_module, SIGNAL( sigRosShutdown() ), this, SLOT( sltRosShutdown() ) );
    connect( qnode_module, SIGNAL( sigcurrentPose( QList< double > ) ), this, SLOT( sltcurrentPose( QList< double > ) ) );
    connect( qnode_module, SIGNAL( sigtargetSpeed( QList< double > ) ), this, SLOT( slttargetSpeed( QList< double > ) ) );
    connect( qnode_module, SIGNAL( sigonGoalReached() ), this, SLOT( sltonGoalReached() ) );
    connect( qnode_module, SIGNAL( sigonNavStatus( int ) ), this, SLOT( sltonNavStatus( int ) ) );
    connect( qnode_module, SIGNAL( sigonLocalizationLose() ), this, SLOT( sigonLocalizationLose() ) );
    //  qnode_module->start();
    qNodelet->init( "qnode_qt", {}, {} );

    //定时器初始化
    QTimer* timer50ms = new QTimer();
    timer50ms->setTimerType( Qt::PreciseTimer );
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( slt50msTimer() ) );
    //    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( slt50msTimer_1() )
    //    );
    timer50ms->start( Param::predict_time );

    /***************************************日志***************************************/
    QTimer* timer_logger = new QTimer();
    timer_logger->setTimerType( Qt::CoarseTimer );
    connect( timer_logger, SIGNAL( timeout() ), this, SLOT( sigLog() ) );
    qDebug() << Param::log_timer;
    timer_logger->start( Param::log_timer );

    qnode_connect_time.setTimerType( Qt::PreciseTimer );
    connect( &qnode_connect_time, &QTimer::timeout, this, [=]() -> void {
        //节点失去连接
        setEmergencyStop( true );
        sigSendTips( "错误，定位获取超时保护触发" );
        qnode_connect_time.stop();
    } );
    qnode_connect_time.start( 3000 );
}

Point Navigator::slamPoint() const {
    return slam_point;
}

Point Navigator::curPoint() const {
    return cur_point;
}

bool Navigator::isEmergencyStop() const {
    return is_emergency_stop;
}

void Navigator::setEmergencyStop( bool isEmergencyStop ) {
    is_emergency_stop = isEmergencyStop;
}

bool Navigator::isAutoModeEnable() const {
    return is_auto_mode_enable;
}

void Navigator::setAutoModeEnable( bool isAutoModeEnable ) {
    is_auto_mode_enable = isAutoModeEnable;
}

Navigator::NaviStep Navigator::getSlamNaviStep() const {
    return slamNaviStep;
}

bool Navigator::isObstacleFunctionEnable() const {
    return is_obstacle_function_enable;
}

bool Navigator::isForward() const {
    return sub_route.isforward;
}
